#include "AUVMainWindow.h"
#include "ui_AUVMainWindow.h"
#include <QRandomGenerator>
#include <cmath>  // 用于数学计算

AUVMainWindow::AUVMainWindow(QWidget *parent)
    : QMainWindow(parent)
    , ui(new Ui::AUVMainWindow)
{
    ui->setupUi(this);

    // 初始化轨迹显示场景
    mapScene = new QGraphicsScene(this);
    ui->mapView->setScene(mapScene);

    // 初始化TCP客户端
    tcpClient = new QTcpSocket(this);
    connect(tcpClient, &QTcpSocket::connected, this, &AUVMainWindow::onConnected);
    connect(tcpClient, &QTcpSocket::disconnected, this, &AUVMainWindow::onDisconnected);
    connect(tcpClient, &QTcpSocket::errorOccurred, this, &AUVMainWindow::onError);

    // 连接到用户台
    tcpClient->connectToHost("127.0.0.1", 54321);

    // 初始化按钮连接
    connect(ui->autoModeButton, &QPushButton::clicked, this, &AUVMainWindow::onAutoModeClicked);
    connect(ui->manualModeButton, &QPushButton::clicked, this, &AUVMainWindow::onManualModeClicked);
    connect(ui->forwardButton, &QPushButton::clicked, this, &AUVMainWindow::onForwardClicked);
    connect(ui->backwardButton, &QPushButton::clicked, this, &AUVMainWindow::onBackwardClicked);
    connect(ui->turnLeftButton, &QPushButton::clicked, this, &AUVMainWindow::onTurnLeftClicked);
    connect(ui->turnRightButton, &QPushButton::clicked, this, &AUVMainWindow::onTurnRightClicked);

    // 初始化当前轨迹点位置
    currentPoint = QPointF(ui->mapView->width() / 2, ui->mapView->height() / 2);
    sendTrackPoint(currentPoint);  // 发送初始位置
}

AUVMainWindow::~AUVMainWindow() {
    tcpClient->disconnectFromHost();
    delete ui;
}

void AUVMainWindow::sendTrackPoint(const QPointF &point) {
    // 绘制轨迹点
    mapScene->addEllipse(point.x() - 2, point.y() - 2, 4, 4, QPen(Qt::blue), QBrush(Qt::blue));

    // 如果有前一个点，绘制连接线
    if (!trackPoints.isEmpty()) {
        QPointF lastPoint = trackPoints.last();
        mapScene->addLine(lastPoint.x(), lastPoint.y(), point.x(), point.y(), QPen(Qt::blue, 2));
    }

    trackPoints.append(point);

    // 发送轨迹点到用户台
    if (tcpClient->state() == QAbstractSocket::ConnectedState) {
        tcpClient->write(QString("TRACK_POINT %1 %2\n").arg(point.x()).arg(point.y()).toUtf8());
    }
}

void AUVMainWindow::onAutoModeClicked() {
    tcpClient->write("MODE_AUTO\n");
    ui->statusLabel->setText("Status: Auto Mode");

    // 获取自定义目标点
    QString coordinateText = ui->customCoordinateLineEdit->text();
    QStringList coordinates = coordinateText.split(',');
    if (coordinates.size() != 2) {
        ui->statusLabel->setText("Error: Invalid coordinate input. Use format 'x,y'.");
        return;
    }

    bool okX, okY;
    qreal targetX = coordinates[0].toDouble(&okX);
    qreal targetY = coordinates[1].toDouble(&okY);
    if (!okX || !okY) {
        ui->statusLabel->setText("Error: Invalid coordinate input.");
        return;
    }

    QPointF targetPoint(targetX, targetY);
    generateRandomPathToTarget(targetPoint);
}

void AUVMainWindow::generateRandomPathToTarget(const QPointF &targetPoint) {
    QPointF current = currentPoint;  // 当前点
    QPointF target = targetPoint;    // 目标点
    qreal stepSize = 5.0;            // 每步移动距离
    int steps = 50;                  // 总步数

    for (int i = 0; i < steps; i++) {
        QPointF direction = target - current;  // 计算方向向量
        qreal angle = atan2(direction.y(), direction.x()) * 180 / M_PI;  // 计算方向角度
        currentAngle = angle;  // 更新当前方向角度

        // 添加随机偏移（使用 C++ 标准库生成随机数）
        qreal dx = stepSize * cos(angle * M_PI / 180) + QRandomGenerator::global()->bounded(-5, 5);
        qreal dy = stepSize * sin(angle * M_PI / 180) + QRandomGenerator::global()->bounded(-5, 5);

        QPointF newPoint(current.x() + dx, current.y() + dy);
        sendTrackPoint(newPoint);  // 发送轨迹点
        current = newPoint;        // 更新当前点
    }

    // 确保最终到达目标点
    sendTrackPoint(target);
    currentPoint = target;  // 更新当前点为目标点
    currentAngle = atan2(target.y() - current.y(), target.x() - current.x()) * 180 / M_PI;  // 更新最终方向角度
}

void AUVMainWindow::onManualModeClicked() {
    tcpClient->write("MODE_MANUAL\n");
    ui->statusLabel->setText("Status: Manual Mode");
}

void AUVMainWindow::onForwardClicked() {
    qreal radians = currentAngle * M_PI / 180;
    QPointF newPoint(currentPoint.x() + 10 * cos(radians), currentPoint.y() + 10 * sin(radians));
    sendTrackPoint(newPoint);
    currentPoint = newPoint;
}

void AUVMainWindow::onBackwardClicked() {
    qreal radians = currentAngle * M_PI / 180;
    QPointF newPoint(currentPoint.x() - 10 * cos(radians), currentPoint.y() - 10 * sin(radians));
    sendTrackPoint(newPoint);
    currentPoint = newPoint;
}

void AUVMainWindow::onTurnLeftClicked() {
    currentAngle -= 10;  // 左转10度
    if (currentAngle < 0) currentAngle += 360;  // 保持角度在0-360范围内
    sendTrackPoint(currentPoint);  // 重新发送当前点
}

void AUVMainWindow::onTurnRightClicked() {
    currentAngle += 10;  // 右转10度
    if (currentAngle >= 360) currentAngle -= 360;  // 保持角度在0-360范围内
    sendTrackPoint(currentPoint);  // 重新发送当前点
}

void AUVMainWindow::onConnected() {
    ui->statusLabel->setText("Connected to user interface.");
}

void AUVMainWindow::onDisconnected() {
    ui->statusLabel->setText("Disconnected from user interface.");
}

void AUVMainWindow::onError() {
    ui->statusLabel->setText("Error: " + tcpClient->errorString());
}
